static class Math{
  
  static double COMPTOL = 0.0000000001;

      /// <summary>
      /// Converting degrees to radians
      /// </summary>
      /// <param name="angle">Degrees input</param>
      /// <returns>Radians output</returns>
      public static double DegreeToRadian(double angle)
      {
          return PI * angle / 180.0;
      }
      /// <summary>
      /// Converting degrees to radians
      /// </summary>
      /// <param name="angle">Degrees input</param>
      /// <returns>Radians output</returns>
      public static double DegreeToRadian(float angle)
      {
          return PI * (double)angle / 180.0f;
      }

      /// <summary>
      /// Converting radians to degrees
      /// </summary>
      /// <param name="angle">Radians input</param>
      /// <returns>Degrees output</returns>
      public static double RadianToDegree(double angle)
      {
          return angle * (180.0 / PI);
      }
      /// <summary>
      /// Converting radians to degrees
      /// </summary>
      /// <param name="angle">Radians input</param>
      /// <returns>Degrees output</returns>
      public static double RadianToDegree(float angle)
      {
          return (double)angle * (180.0 / PI);
      }

      /// <summary>
      /// Multiplies t1 with t2.
      /// </summary>
      /// <param name="t1">Double[,] 4x4 matrix</param>
      /// <param name="t2">Double[,] 4x4 matrix</param>
      /// <returns>Double[,] 4x4 matrix</returns>
      public static double[][] Tform_mult(double[][] t1, double[][] t2)
      {
          double[][] t3 = new double[4] [4];



          t3 = ConvertFloatToDouble(Uni2mtr());

          //t3[0, 0] = t2[0, 0] * t1[0, 0] + t2[1, 0] * t1[0, 1] + t2[2, 0] * t1[0, 2] + t2[3, 0] * t1[0, 3];
          //t3[1, 0] = t2[0, 0] * t1[1, 0] + t2[1, 0] * t1[1, 1] + t2[2, 0] * t1[1, 2] + t2[3, 0] * t1[1, 3];
          //t3[2, 0] = t2[0, 0] * t1[2, 0] + t2[1, 0] * t1[2, 1] + t2[2, 0] * t1[2, 2] + t2[3, 0] * t1[2, 3];
          //t3[3, 0] = t2[0, 0] * t1[3, 0] + t2[1, 0] * t1[3, 1] + t2[2, 0] * t1[3, 2] + t2[3, 0] * t1[3, 3];

          //t3[0, 1] = t2[0, 1] * t1[0, 0] + t2[1, 1] * t1[0, 1] + t2[2, 1] * t1[0, 2] + t2[3, 1] * t1[0, 3];
          //t3[1, 1] = t2[0, 1] * t1[1, 0] + t2[1, 1] * t1[1, 1] + t2[2, 1] * t1[1, 2] + t2[3, 1] * t1[1, 3];
          //t3[2, 1] = t2[0, 1] * t1[2, 0] + t2[1, 1] * t1[2, 1] + t2[2, 1] * t1[2, 2] + t2[3, 1] * t1[2, 3];
          //t3[3, 1] = t2[0, 1] * t1[3, 0] + t2[1, 1] * t1[3, 1] + t2[2, 1] * t1[3, 2] + t2[3, 1] * t1[3, 3];

          //t3[0, 2] = t2[0, 2] * t1[0, 0] + t2[1, 2] * t1[0, 1] + t2[2, 2] * t1[0, 2] + t2[3, 2] * t1[0, 3];
          //t3[1, 2] = t2[0, 2] * t1[1, 0] + t2[1, 2] * t1[1, 1] + t2[2, 2] * t1[1, 2] + t2[3, 2] * t1[1, 3];
          //t3[2, 2] = t2[0, 2] * t1[2, 0] + t2[1, 2] * t1[2, 1] + t2[2, 2] * t1[2, 2] + t2[3, 2] * t1[2, 3];
          //t3[3, 2] = t2[0, 2] * t1[3, 0] + t2[1, 2] * t1[3, 1] + t2[2, 2] * t1[3, 2] + t2[3, 2] * t1[3, 3];

          //t3[0, 3] = t2[0, 3] * t1[0, 0] + t2[1, 3] * t1[0, 1] + t2[2, 3] * t1[0, 2] + t2[3, 3] * t1[0, 3];
          //t3[1, 3] = t2[0, 3] * t1[1, 0] + t2[1, 3] * t1[1, 1] + t2[2, 3] * t1[1, 2] + t2[3, 3] * t1[1, 3];
          //t3[2, 3] = t2[0, 3] * t1[2, 0] + t2[1, 3] * t1[2, 1] + t2[2, 3] * t1[2, 2] + t2[3, 3] * t1[2, 3];
          //t3[3, 3] = t2[0, 3] * t1[3, 0] + t2[1, 3] * t1[3, 1] + t2[2, 3] * t1[3, 2] + t2[3, 3] * t1[3, 3];

    t3[0][0] = t1[0][0] * t2[0][0] + t1[0][1] * t2[1][0] + t1[0][2] * t2[2][0] + t1[0][3] * t2[3][0];
          t3[0][1] = t1[0][0] * t2[0][1] + t1[0][1] * t2[1][1] + t1[0][2] * t2[2][1] + t1[0][3] * t2[3][1];
          t3[0][2] = t1[0][0] * t2[0][2] + t1[0][1] * t2[1][2] + t1[0][2] * t2[2][2] + t1[0][3] * t2[3][2];
          t3[0][3] = t1[0][0] * t2[0][3] + t1[0][1] * t2[1][3] + t1[0][2] * t2[2][3] + t1[0][3] * t2[3][3];

          t3[1][0] = t1[1][0] * t2[0][0] + t1[1][1] * t2[1][0] + t1[1][2] * t2[2][0] + t1[1][3] * t2[3][0];
          t3[1][1] = t1[1][0] * t2[0][1] + t1[1][1] * t2[1][1] + t1[1][2] * t2[2][1] + t1[1][3] * t2[3][1];
          t3[1][2] = t1[1][0] * t2[0][2] + t1[1][1] * t2[1][2] + t1[1][2] * t2[2][2] + t1[1][3] * t2[3][2];
          t3[1][3] = t1[1][0] * t2[0][3] + t1[1][1] * t2[1][3] + t1[1][2] * t2[2][3] + t1[1][3] * t2[3][3];

          t3[2][0] = t1[2][0] * t2[0][0] + t1[2][1] * t2[1][0] + t1[2][2] * t2[2][0] + t1[2][3] * t2[3][0];
          t3[2][1] = t1[2][0] * t2[0][1] + t1[2][1] * t2[1][1] + t1[2][2] * t2[2][1] + t1[2][3] * t2[3][1];
          t3[2][2] = t1[2][0] * t2[0][2] + t1[2][1] * t2[1][2] + t1[2][2] * t2[2][2] + t1[2][3] * t2[3][2];
          t3[2][3] = t1[2][0] * t2[0][3] + t1[2][1] * t2[1][3] + t1[2][2] * t2[2][3] + t1[2][3] * t2[3][3];

          t3[3][0] = t1[3][3] * t2[3][0];
          t3[3][1] = t1[3][3] * t2[3][1];
          t3[3][2] = t1[3][3] * t2[3][2];
          t3[3][3] = t1[3][3] * t2[3][3];

          return t3;
      }
      /// <summary>
      /// Multiplies t1 with t2.
      /// </summary>
      /// <param name="t1">Double[,] 4x4 matrix</param>
      /// <param name="t2">Double[,] 4x4 matrix</param>
      /// <returns>Double[,] 4x4 matrix</returns>
      public static float[][] Tform_mult(float[][] t1, float[][] t2)
      {
          float[][] t3 = new float[4][4];

          t3 = Uni2mtr();

          t3[0][0] = t1[0][0] * t2[0][0] + t1[0][1] * t2[1][0] + t1[0][2] * t2[2][0] + t1[0][3] * t2[3][0];
          t3[0][1] = t1[0][0] * t2[0][1] + t1[0][1] * t2[1][1] + t1[0][2] * t2[2][1] + t1[0][3] * t2[3][1];
          t3[0][2] = t1[0][0] * t2[0][2] + t1[0][1] * t2[1][2] + t1[0][2] * t2[2][2] + t1[0][3] * t2[3][2];
          t3[0][3] = t1[0][0] * t2[0][3] + t1[0][1] * t2[1][3] + t1[0][2] * t2[2][3] + t1[0][3] * t2[3][3];

          t3[1][0] = t1[1][0] * t2[0][0] + t1[1][1] * t2[1][0] + t1[1][2] * t2[2][0] + t1[1][3] * t2[3][0];
          t3[1][1] = t1[1][0] * t2[0][1] + t1[1][1] * t2[1][1] + t1[1][2] * t2[2][1] + t1[1][3] * t2[3][1];
          t3[1][2] = t1[1][0] * t2[0][2] + t1[1][1] * t2[1][2] + t1[1][2] * t2[2][2] + t1[1][3] * t2[3][2];
          t3[1][3] = t1[1][0] * t2[0][3] + t1[1][1] * t2[1][3] + t1[1][2] * t2[2][3] + t1[1][3] * t2[3][3];

          t3[2][0] = t1[2][0] * t2[0][0] + t1[2][1] * t2[1][0] + t1[2][2] * t2[2][0] + t1[2][3] * t2[3][0];
          t3[2][1] = t1[2][0] * t2[0][1] + t1[2][1] * t2[1][1] + t1[2][2] * t2[2][1] + t1[2][3] * t2[3][1];
          t3[2][2] = t1[2][0] * t2[0][2] + t1[2][1] * t2[1][2] + t1[2][2] * t2[2][2] + t1[2][3] * t2[3][2];
          t3[2][3] = t1[2][0] * t2[0][3] + t1[2][1] * t2[1][3] + t1[2][2] * t2[2][3] + t1[2][3] * t2[3][3];

          t3[3][0] = t1[3][3] * t2[3][0];
          t3[3][1] = t1[3][3] * t2[3][1];
          t3[3][2] = t1[3][3] * t2[3][2];
          t3[3][3] = t1[3][3] * t2[3][3];

          return t3;
      }

      /// <summary>
      /// Determinant of a t
      /// </summary>
      /// <param name="t">Double[,] 4x4 matrix</param>
      /// <returns>float Determinant</returns>
      public static float Tform_det(double[][] t)
      {
          float dum1, dum2, dum3, dum4;

          /* Determinant for element t0,0 */

          dum1 = (float)(t[1][1] * t[2][2] * t[3][3] +
                 t[1][2] * t[2][3] * t[3][1] +
                 t[1][3] * t[2][1] * t[3][2] -
                 t[1][1] * t[2][3] * t[3][2] -
                 t[1][2] * t[2][1] * t[3][3] -
                 t[1][3] * t[2][2] * t[3][1]);

          dum1 = (float)t[0][0] * dum1;


          /* Determinant for element t0,1 */

          dum2 = (float)(t[1][0] * t[2][2] * t[3][3] +
                 t[1][2] * t[1][3] * t[3][0] +
                 t[1][3] * t[2][0] * t[3][2] -
                 t[1][0] * t[2][3] * t[3][2] -
                 t[1][2] * t[2][0] * t[3][3] -
                 t[1][3] * t[2][2] * t[3][0]);

          dum2 = -(float)t[0][1] * dum2;


          /* Determinant for element t0,2 */

          dum3 = (float)(t[1][0] * t[2][1] * t[3][3] +
                 t[1][1] * t[2][3] * t[3][0] +
                 t[1][3] * t[2][0] * t[3][1] -
                 t[1][0] * t[2][3] * t[3][1] -
                 t[1][1] * t[2][0] * t[3][3] -
                 t[1][3] * t[2][1] * t[3][0]);

          dum3 = (float)t[0][2] * dum3;


          /* Determinant for element t0,3 */

          dum4 = (float)(t[1][1] * t[2][2] * t[3][0] +
                 t[1][2] * t[2][0] * t[3][1] +
                 t[1][0] * t[2][1] * t[3][2] -
                 t[1][1] * t[2][0] * t[3][2] -
                 t[1][2] * t[2][1] * t[3][0] -
                 t[1][0] * t[2][2] * t[3][1]);

          dum4 = -(float)t[0][3] * dum4;

          return (dum1 + dum2 + dum3 + dum4);


          // if ( ABS(*pdet) < TOL1 ) return(erpush("GE6144","GEtform_det"));

          // return(0);
          //}
      }
      /// <summary>
      /// Determinant of a t
      /// </summary>
      /// <param name="t">Double[,] 4x4 matrix</param>
      /// <returns>float Determinant</returns>
      public static float Tform_det(float[][] t)
      {
          float dum1, dum2, dum3, dum4;

          /* Determinant for element t0,0 */

          dum1 = (t[1][1] * t[2][2] * t[3][3] +
                 t[1][2] * t[2][3] * t[3][1] +
                 t[1][3] * t[2][1] * t[3][2] -
                 t[1][1] * t[2][3] * t[3][2] -
                 t[1][2] * t[2][1] * t[3][3] -
                 t[1][3] * t[2][2] * t[3][1]);

          dum1 = t[0][0] * dum1;


          /* Determinant for element t0,1 */

          dum2 = (t[1][0] * t[2][2] * t[3][3] +
                 t[1][2] * t[1][3] * t[3][0] +
                 t[1][3] * t[2][0] * t[3][2] -
                 t[1][0] * t[2][3] * t[3][2] -
                 t[1][2] * t[2][0] * t[3][3] -
                 t[1][3] * t[2][2] * t[3][0]);

          dum2 = -t[0][1] * dum2;


          /* Determinant for element t0,2 */

          dum3 = (t[1][0] * t[2][1] * t[3][3] +
                 t[1][1] * t[2][3] * t[3][0] +
                 t[1][3] * t[2][0] * t[3][1] -
                 t[1][0] * t[2][3] * t[3][1] -
                 t[1][1] * t[2][0] * t[3][3] -
                 t[1][3] * t[2][1] * t[3][0]);

          dum3 = t[0][2] * dum3;


          /* Determinant for element t0,3 */

          dum4 = (t[1][1] * t[2][2] * t[3][0] +
                 t[1][2] * t[2][0] * t[3][1] +
                 t[1][0] * t[2][1] * t[3][2] -
                 t[1][1] * t[2][0] * t[3][2] -
                 t[1][2] * t[2][1] * t[3][0] -
                 t[1][0] * t[2][2] * t[3][1]);

          dum4 = -t[0][3] * dum4;

          return (dum1 + dum2 + dum3 + dum4);


          // if ( ABS(*pdet) < TOL1 ) return(erpush("GE6144","GEtform_det"));

          // return(0);
          //}
      }

      /// <summary>
      /// Calculates the inverse of t1
      /// </summary>
      /// <param name="t1">Double[,] 4x4 matrix</param>
      /// <returns>Double[,] 4x4 matrix</returns>
      public static double[][] Tform_inv(double[][] t1)
      {
          double[][] t2 = new double[4][4];

          t2 = ConvertFloatToDouble(Uni2mtr());

          /* Rows and columns change place in the upper left 3x3 matrix */

          t2[0][0] = t1[0][0];
          t2[0][1] = t1[1][0];
          t2[0][2] = t1[2][0];

          t2[1][0] = t1[0][1];
          t2[1][1] = t1[1][1];
          t2[1][2] = t1[2][1];

          t2[2][0] = t1[0][2];
          t2[2][1] = t1[1][2];
          t2[2][2] = t1[2][2];

          /* The bottom row 0 0 0 1  equal to the original matrix  */

          t2[3][0] = t1[3][0];
          t2[3][1] = t1[3][1];
          t2[3][2] = t1[3][2];
          t2[3][3] = t1[3][3];

          /* Translation coefficients for the inverse with multiplication */
          /* of the inverse (transposed) matrix and the original trans-   */
          /* slation coefficients                                         */

          t2[0][3] = -(t2[0][0] * t1[0][3] +
                       t2[0][1] * t1[1][3] +
                       t2[0][2] * t1[2][3]);

          t2[1][3] = -(t2[1][0] * t1[0][3] +
                       t2[1][1] * t1[1][3] +
                       t2[1][2] * t1[2][3]);

          t2[2][3] = -(t2[2][0] * t1[0][3] +
                       t2[2][1] * t1[1][3] +
                       t2[2][2] * t1[2][3]);

          return t2;
      }
      /// <summary>
      /// Calculates the inverse of t1
      /// </summary>
      /// <param name="t1">Double[,] 4x4 matrix</param>
      /// <returns>Double[,] 4x4 matrix</returns>
      public static float[][] Tform_inv(float[][] t1)
      {
          float[][] t2 = new float[4][4];

          t2 = Uni2mtr();

          /* Rows and columns change place in the upper left 3x3 matrix */

          t2[0][0] = t1[0][0];
          t2[0][1] = t1[1][0];
          t2[0][2] = t1[2][0];

          t2[1][0] = t1[0][1];
          t2[1][1] = t1[1][1];
          t2[1][2] = t1[2][1];

          t2[2][0] = t1[0][2];
          t2[2][1] = t1[1][2];
          t2[2][2] = t1[2][2];

          /* The bottom row 0 0 0 1  equal to the original matrix  */

          t2[3][0] = t1[3][0];
          t2[3][1] = t1[3][1];
          t2[3][2] = t1[3][2];
          t2[3][3] = t1[3][3];

          /* Translation coefficients for the inverse with multiplication */
          /* of the inverse (transposed) matrix and the original trans-   */
          /* slation coefficients                                         */

          t2[0][3] = -(t2[0][0] * t1[0][3] +
                       t2[0][1] * t1[1][3] +
                       t2[0][2] * t1[2][3]);

          t2[1][3] = -(t2[1][0] * t1[0][3] +
                       t2[1][1] * t1[1][3] +
                       t2[1][2] * t1[2][3]);

          t2[2][3] = -(t2[2][0] * t1[0][3] +
                       t2[2][1] * t1[1][3] +
                       t2[2][2] * t1[2][3]);

          return t2;
      }

      /// <summary>
      ///  Copy of a 4x4 transformation matrices
      /// </summary>
      /// <param name="m1">double[,] 4x4 matrix</param>
      /// <returns>double[,] 4x4 matrix</returns>
      public static double[][] Cpy_mtr(double[][] m1)
      {
          double[][] m = new double[4][4];

          m[0][0] = m1[0][0]; m[0][1] = m1[0][1]; m[0][2] = m1[0][2]; m[0][3] = m1[0][3];
          m[1][0] = m1[1][0]; m[1][1] = m1[1][1]; m[1][2] = m1[1][2]; m[1][3] = m1[1][3];
          m[2][0] = m1[2][0]; m[2][1] = m1[2][1]; m[2][2] = m1[2][2]; m[2][3] = m1[2][3];
          m[3][0] = m1[3][0]; m[3][1] = m1[3][1]; m[3][2] = m1[3][2]; m[3][3] = m1[3][3];

          return m;
      }
      /// <summary>
      ///  Copy of a 4x4 transformation matrices
      /// </summary>
      /// <param name="m1">float[,] 4x4 matrix</param>
      /// <returns>float[,] 4x4 matrix</returns>
      public static float[][] Cpy_mtr(float[][] m1)
      {
          float[][] m = new float[4][4];

          m[0][0] = m1[0][0]; m[0][1] = m1[0][1]; m[0][2] = m1[0][2]; m[0][3] = m1[0][3];
          m[1][0] = m1[1][0]; m[1][1] = m1[1][1]; m[1][2] = m1[1][2]; m[1][3] = m1[1][3];
          m[2][0] = m1[2][0]; m[2][1] = m1[2][1]; m[2][2] = m1[2][2]; m[2][3] = m1[2][3];
          m[3][0] = m1[3][0]; m[3][1] = m1[3][1]; m[3][2] = m1[3][2]; m[3][3] = m1[3][3];

          return m;
      }

      /// <summary>
      /// Position to transformation matrices
      /// </summary>
      /// <param name="x">X position</param>
      /// <param name="y">Y position</param>
      /// <param name="z">Z position</param>
      /// <returns>double[,] 4x4 matrix</returns>
      /// 
      public static double[][] Pos2mtr(double x, double y, double z)
      {
          double[] v = new double[] { x, y, z };
          double[][] m = new double[4][4];


          m[0][0] = 1; m[0][1] = 0; m[0][2] = 0; m[0][3] = v[0];
          m[1][0] = 0; m[1][1] = 1; m[1][2] = 0; m[1][3] = v[1];
          m[2][0] = 0; m[2][1] = 0; m[2][2] = 1; m[2][3] = v[2];
          m[3][0] = 0; m[3][1] = 0; m[3][2] = 0; m[3][3] = 1;

          return m;
      }
      /// <summary>
      /// Position to transformation matrices
      /// </summary>
      /// <param name="v">Positions vector</param>
      /// <returns>double[,] 4x4 matrix</returns>
      /// 
      public static double[][] Pos2mtr(double[] v)
      {
          double[][] m = new double[4][4];


          m[0][0] = 1; m[0][1] = 0; m[0][2] = 0; m[0][3] = v[0];
          m[1][0] = 0; m[1][1] = 1; m[1][2] = 0; m[1][3] = v[1];
          m[2][0] = 0; m[2][1] = 0; m[2][2] = 1; m[2][3] = v[2];
          m[3][0] = 0; m[3][1] = 0; m[3][2] = 0; m[3][3] = 1;

          return m;
      }

      /// <summary>
      /// Create a transformation matrix by x,y,z,rotx,roty,rotz
      /// </summary>
      /// <param name="t">x,y,z,rotx,roty,rotz float[6]</param>
      /// <returns>double[,] 4x4 matrix</returns>
      public static double[][] Pos_Rot2m(double[] t)
      {
          double[][] m = new double[4][4];
          m = ConvertFloatToDouble(Uni2mtr());
          double[][] tempm = new double[4][4];
          tempm = ConvertFloatToDouble(Uni2mtr());

          /*
          ***Create unrotated system 
          */
          m[0][0] = 1; m[0][1] = 0; m[0][2] = 0; m[0][3] = t[0];
          m[1][0] = 0; m[1][1] = 1; m[1][2] = 0; m[1][3] = t[1];
          m[2][0] = 0; m[2][1] = 0; m[2][2] = 1; m[2][3] = t[2];
          m[3][0] = 0; m[3][1] = 0; m[3][2] = 0; m[3][3] = 1;
          /*
          ***Rotate around X.
          */
          if (t[3] != 0.0f)
          {
              tempm[0][0] = 1; tempm[0][1] = 0; tempm[0][2] = 0; tempm[0][3] = 0;
              tempm[1][0] = 0; tempm[1][1] = (double)cos((float)(DegreeToRadian(t[3]))); tempm[1][2] = -(double)sin((float)(DegreeToRadian(t[3]))); tempm[1][3] = 0;
              tempm[2][0] = 0; tempm[2][1] = (double)sin((float)(DegreeToRadian(t[3]))); tempm[2][2] = (double)cos((float)(DegreeToRadian(t[3]))); tempm[2][3] = 0;
              tempm[3][0] = 0; tempm[3][1] = 0; tempm[3][2] = 0; tempm[3][3] = 1;
              m = Tform_mult(m,tempm);
          }
          /*
          ***Rotate around Y.
          */
          if (t[5] != 0.0f)
          {
              tempm[0][0] = (double)cos((float)(DegreeToRadian(t[4]))); tempm[0][1] = 0; tempm[0][2] = (double)sin((float)(DegreeToRadian(t[4]))); tempm[0][3] = 0;
              tempm[1][0] = 0; tempm[1][1] = 1; tempm[1][2] = 0; tempm[1][3] = 0;
              tempm[2][0] = -(double)sin((float)(DegreeToRadian(t[4]))); tempm[2][1] = 0; tempm[2][2] = (double)cos((float)(DegreeToRadian(t[4]))); tempm[2][3] = 0;
              tempm[3][0] = 0; tempm[3][1] = 0; tempm[3][2] = 0; tempm[3][3] = 1;
              m = Tform_mult(m,tempm);
          }
          /*
          ***Rotate around Z
          */
          if (t[5] != 0.0f)
          {
              tempm[0][0] = (double)cos((float)(DegreeToRadian(t[5]))); tempm[0][1] = -sin((float)(DegreeToRadian(t[5]))); tempm[0][2] = 0; tempm[0][3] = 0;
              tempm[1][0] = (double)sin((float)(DegreeToRadian(t[5]))); tempm[1][1] = (double)cos((float)(DegreeToRadian(t[5]))); tempm[1][2] = 0; tempm[1][3] = 0;
              tempm[2][0] = 0; tempm[2][1] = 0; tempm[2][2] = 1; tempm[2][3] = 0;
              tempm[3][0] = 0; tempm[3][1] = 0; tempm[3][2] = 0; tempm[3][3] = 1;
              m = Tform_mult(m,tempm);
          }

          return m;
      }
      /// <summary>
      /// Create a transformation matrix by x,y,z,rotx,roty,rotz
      /// </summary>
      /// <param name="t">x,y,z,rotx,roty,rotz float[6]</param>
      /// <returns>float[,] 4x4 matrix</returns>
      public static float[][] Pos_Rot2m(float[] t)
      {
          float[][] m = new float[4][4];
          m = Uni2mtr();

          float[][] tempm = new float[4][4];
          tempm = Uni2mtr();

          /*
          ***Create unrotated system 
          */
          m[0][0] = 1; m[0][1] = 0; m[0][2] = 0; m[0][3] = t[0];
          m[1][0] = 0; m[1][1] = 1; m[1][2] = 0; m[1][3] = t[1];
          m[2][0] = 0; m[2][1] = 0; m[2][2] = 1; m[2][3] = t[2];
          m[3][0] = 0; m[3][1] = 0; m[3][2] = 0; m[3][3] = 1;
          /*
          ***Rotate around X.
          */
          if (t[3] != 0.0f)
          {
              tempm[0][0] = 1; tempm[0][1] = 0; tempm[0][2] = 0; tempm[0][3] = 0;
              tempm[1][0] = 0; tempm[1][1] = cos((float)DegreeToRadian(t[3])); tempm[1][2] = -sin((float)DegreeToRadian(t[3])); tempm[1][3] = 0;
              tempm[2][0] = 0; tempm[2][1] = sin((float)DegreeToRadian(t[3])); tempm[2][2] = cos((float)DegreeToRadian(t[3])); tempm[2][3] = 0;
              tempm[3][0] = 0; tempm[3][1] = 0; tempm[3][2] = 0; tempm[3][3] = 1;
              m = Tform_mult(m,tempm);
          }
          /*
          ***Rotate around Y.
          */
          if (t[4] != 0.0f)
          {
              tempm[0][0] = cos((float)DegreeToRadian(t[4])); tempm[0][1] = 0; tempm[0][2] = sin((float)DegreeToRadian(t[4])); tempm[0][3] = 0;
              tempm[1][0] = 0; tempm[1][1] = 1; tempm[1][2] = 0; tempm[1][3] = 0;
              tempm[2][0] = -sin((float)DegreeToRadian(t[4])); tempm[2][1] = 0; tempm[2][2] = cos((float)DegreeToRadian(t[4])); tempm[2][3] = 0;
              tempm[3][0] = 0; tempm[3][1] = 0; tempm[3][2] = 0; tempm[3][3] = 1;
              m = Tform_mult(m,tempm);
          }
          /*
          ***Rotate around Z
          */
          if (t[5] != 0.0f)
          {
              tempm[0][0] = cos((float)DegreeToRadian(t[5])); tempm[0][1] = -sin((float)DegreeToRadian(t[5])); tempm[0][2] = 0; tempm[0][3] = 0;
              tempm[1][0] = sin((float)DegreeToRadian(t[5])); tempm[1][1] = cos((float)DegreeToRadian(t[5])); tempm[1][2] = 0; tempm[1][3] = 0;
              tempm[2][0] = 0; tempm[2][1] = 0; tempm[2][2] = 1; tempm[2][3] = 0;
              tempm[3][0] = 0; tempm[3][1] = 0; tempm[3][2] = 0; tempm[3][3] = 1;
              m = Tform_mult(m,tempm);
          }

          return m;
      }

      /// <summary>
      /// Rotation x to matrix
      /// </summary>
      /// <param name="a">Rotation round x-axis</param>
      /// <returns>double[,] 4x4 matrix</returns>
      public static double[][] Rotx2mtr(double a)
      {
          double[][] m = new double[4][4];

          m[0][0] = 1; m[0][1] = 0; m[0][2] = 0; m[0][3] = 0;
          m[1][0] = 0; m[1][1] = (double)cos((float)DegreeToRadian(a)); m[1][2] = -(double)sin((float)DegreeToRadian(a)); m[1][3] = 0;
          m[2][0] = 0; m[2][1] = (double)sin((float)DegreeToRadian(a)); m[2][2] = (double)cos((float)DegreeToRadian(a)); m[2][3] = 0;
          m[3][0] = 0; m[3][1] = 0; m[3][2] = 0; m[3][3] = 1;
          return m;
      }
      /// <summary>
      /// Rotation x to matrix
      /// </summary>
      /// <param name="a">Rotation round x-axis</param>
      /// <returns>float[,] 4x4 matrix</returns>
      public static float[][] Rotx2mtr(float a)
      {
          float[][] m = new float[4][4];

          m[0][0] = 1; m[0][1] = 0; m[0][2] = 0; m[0][3] = 0;
          m[1][0] = 0; m[1][1] = cos((float)DegreeToRadian(a)); m[1][2] = -sin((float)DegreeToRadian(a)); m[1][3] = 0;
          m[2][0] = 0; m[2][1] = sin((float)DegreeToRadian(a)); m[2][2] = cos((float)DegreeToRadian(a)); m[2][3] = 0;
          m[3][0] = 0; m[3][1] = 0; m[3][2] = 0; m[3][3] = 1;
          return m;
      }

      /// <summary>
      /// Rotation y to matrix
      /// </summary>
      /// <param name="a">Rotation round y-axis</param>
      /// <returns>double[,] 4x4 matrix</returns>
      public static double[][] Roty2mtr(double a)
      {
          double[][] m = new double[4][4];

          m[0][0] = (double)cos((float)DegreeToRadian(a)); m[0][1] = 0; m[0][2] = (double)sin((float)DegreeToRadian(a)); m[0][3] = 0;
          m[1][0] = 0; m[1][1] = 1; m[1][2] = 0; m[1][3] = 0;
          m[2][0] = -(double)sin((float)DegreeToRadian(a)); m[2][1] = 0; m[2][2] = (double)cos((float)DegreeToRadian(a)); m[2][3] = 0;
          m[3][0] = 0; m[3][1] = 0; m[3][2] = 0; m[3][3] = 1;
          return m;
      }
      /// <summary>
      /// Rotation y to matrix
      /// </summary>
      /// <param name="a">Rotation round y-axis</param>
      /// <returns>float[,] 4x4 matrix</returns>
      public static float[][] Roty2mtr(float a)
      {
          float[][] m = new float[4][4];

          m[0][0] = cos((float)DegreeToRadian(a)); m[0][1] = 0; m[0][2] = sin((float)DegreeToRadian(a)); m[0][3] = 0;
          m[1][0] = 0; m[1][1] = 1; m[1][2] = 0; m[1][3] = 0;
          m[2][0] = -sin((float)DegreeToRadian(a)); m[2][1] = 0; m[2][2] = cos((float)DegreeToRadian(a)); m[2][3] = 0;
          m[3][0] = 0; m[3][1] = 0; m[3][2] = 0; m[3][3] = 1;
          return m;
      }

      /// <summary>
      /// Rotation z to matrix
      /// </summary>
      /// <param name="a">Rotation round z-axis</param>
      /// <returns>double[,] 4x4 matrix</returns>
      public static double[][] Rotz2mtr(double a)
      {
          double[][] m = new double[4][4];

          m[0][0] = (double)cos((float)DegreeToRadian(a)); m[0][1] = -(double)sin((float)DegreeToRadian(a)); m[2][1] = 0; m[0][2] = 0; m[0][3] = 0;
          m[1][0] = (double)sin((float)DegreeToRadian(a)); m[1][1] = (double)cos((float)DegreeToRadian(a)); m[1][2] = 0; m[1][3] = 0;
          m[2][0] = 0; m[2][2] = 1; m[2][3] = 0;
          m[3][0] = 0; m[3][1] = 0; m[3][2] = 0; m[3][3] = 1;
          return m;
      }
      /// <summary>
      /// Rotation z to matrix
      /// </summary>
      /// <param name="a">Rotation round z-axis</param>
      /// <returns>float[,] 4x4 matrix</returns>
      public static float[][] Rotz2mtr(float a)
      {
          float[][] m = new float[4][4];

          m[0][0] = cos((float)DegreeToRadian(a)); m[0][1] = -sin((float)DegreeToRadian(a)); m[0][2] = 0; m[0][3] = 0;
          m[1][0] = sin((float)DegreeToRadian(a)); m[1][1] = cos((float)DegreeToRadian(a)); m[1][2] = 0; m[1][3] = 0;
          m[2][0] = 0; m[2][1] = 0; m[2][2] = 1; m[2][3] = 0;
          m[3][0] = 0; m[3][1] = 0; m[3][2] = 0; m[3][3] = 1;
          return m;
      }

      /// <summary>
      /// Get a unit transformation matrix 
      /// </summary>
      /// <returns>float[,] 4x4 matrix</returns>
      public static float[][] Uni2mtr()
      {
          float[][] m = new float[4][4];

          /*
          ***Create unrotated system 
          */
          m[0][0] = 1; m[0][1] = 0; m[0][2] = 0; m[0][3] = 0;
          m[1][0] = 0; m[1][1] = 1; m[1][2] = 0; m[1][3] = 0;
          m[2][0] = 0; m[2][1] = 0; m[2][2] = 1; m[2][3] = 0;
          m[3][0] = 0; m[3][1] = 0; m[3][2] = 0; m[3][3] = 1;

          return m;
      }
      
      /// <summary>
        /// Convert a 4x4 matrix of type float to double
        /// </summary>
        /// <param name="m">float[,] 4x4 matrix</param>
        /// <returns>double[,] 4x4 matrix</returns>
        public static double[][] ConvertFloatToDouble(float[][] m)
        {
            double[][] d = new double[4][4];

            for (int i = 0; i < 4; i++)
            {
                for (int j = 0; j < 4; j++)
                {
                    d[i][j] = (double)m[i][j];
                }
            }

            return d;
        }
        /// <summary>
        /// Convert a 4x4 matrix of type double to float
        /// </summary>
        /// <param name="m">double[,] 4x4 matrix</param>
        /// <returns>float[,] 4x4 matrix</returns>
        public static float[][] ConvertDoubleToFloat(double[][] m)
        {
            float[][] f = new float[4][4];

            for (int i = 0; i < 4; i++)
            {
                for (int j = 0; j < 4; j++)
                {
                    f[i][j] = (float)m[i][j];
                }
            }

            return f;
        }
}
